/* 
相位输出范围0~2π，相位0对应正弦最大值（即0对应幅值1pu）
参考《基于 SOGI 的谐波条件下同步信号提取方法研究》

对外接口：对外接口：_Creat(),_Cal()

 */

#include <iostream>
#include <tuple>
#include <utility>
#include <cmath>
#include <algorithm>

#include "../include/_DSOGI_PNSC_FLL_PLL.h"

#define π 3.141592653589793238462643383279


//静止转同步坐标系
static auto  αβ2dq( floatx α,floatx β,floatx θ )
{
    auto cost = cos(θ);
    auto sint = sin(θ);
    auto d = cost*α + sint*β;
    auto q = -sint*α + cost*β;     
    return std::make_tuple(d,q);     
}

//求正交幅值
static auto abs( floatx a ,floatx b )
{
    return sqrt( a*a + b*b );
}

//循环积分，后向欧拉，输出在某一区间
static auto 积分_循环( floatx x ,floatx Ts ,floatx lower,floatx upper,floatx y_n_1)
{   
    //y_n_1:上一次输出值
    auto yt = y_n_1;
    yt += Ts*x;

    auto d = fabs(upper-lower);

    if (yt < lower)
    {
        yt += d;
    }
    if (yt > upper)
    {
        yt -= d;
    }
    return yt;
}

//积分并限幅，后向欧拉
static auto 积分并限幅( floatx x ,floatx Ts ,floatx lower,floatx upper,floatx y_n_1)
{   //y_n_1:上一次输出值
    auto yt = y_n_1;
    yt += Ts*x;    

    if (yt < lower) yt = lower;    
    if (yt > upper) yt = upper;
    
    return yt;
}


//积分器，后向欧拉
static auto 积分( floatx x ,floatx Ts ,floatx y_n_1)
{   
    //y_n_1:上一次输出值
    auto yt = y_n_1;
    yt += Ts*x;  
    return yt;   
}

//低通滤波器，后向欧拉
static auto 滤波_低通( floatx x , floatx Ts ,floatx f , floatx y_n_1 )
{
    //y_n_1:上一次输出值，f:滤波频率
    auto T = 1/(2*π*f);
    auto TTs =  T + Ts;
    auto y = y_n_1 * T/TTs + x * Ts/TTs;
    return y ;
}

//PI调节器，积分采用后向欧拉,updata非零时复位   
static void PI计算( PI_Struct *pi ,floatx x ,int update , floatx Ts )
{
    if(update){
        //非零复位
        pi->y = pi->y_init;
        pi->yi = 0;
    }
    else{
        //不复位
        auto yp = pi->kp * x;
        auto yi = pi->yi + pi->ki * Ts * x ;//积分采用后向欧拉
        auto yt = yp +yi;

        //当模块组件之和超出输出限制且积分器输出与模块输入具有相同的符号时，积分停止;
        //当模块组件之和超出输出限制且积分器输出与模块输入具有相反的符号时，积分恢复;
        //钳位有时称为条件积分;
        //对于饱和时间相对较小的被控对象，钳位可能很有用；但对于饱和时间较大的被控对象，它可能产生不良的瞬时响应。
        auto 符号 = ( x>0&&yi>0 )||( x<0&&yi<0 );
        if (yt > pi->upper){
            yt = pi->upper;
            if(符号) yi = pi->yi;
        }
        if (yt < pi->lower) {
            yt = pi->lower;
            if(符号) yi = pi->yi;
        }
        pi->yi = yi;
        pi->y = yt;      
    }

    
/*     auto y = pi->out + pi->ki*Ts*x + pi->kp*(x - pi->in);   
    pi->in = x;
    
    if( y > pi->upper ) {
        y = pi->upper;
    }
    if( y < pi->lower ) {
        y = pi->lower;
    }
    pi->out = y; */


    /* s->y = s->y_n_1 + P * (u - s->u_n_1) + T * I * u;
    if(u < 0)
        s->y = 0;

    if (s->y > max)
        s->y = max;
    if (s->y < min)
        s->y = min;

    s->y_n_1 = s->y;
    s->u_n_1 = u;
    return s->y; */

}

//SOGI正交信号生成，无直流抑制
static void SOGI正交信号生成( SOGI_DCnoRejection *sogi ,floatx x ,floatx w0 , floatx Ts ){
    //SOGI 对象数据更新在函数最后

    auto ein  = x - sogi->X1 ;
    //auto err0 = sogi->k0 *ein;    
    auto errk  = sogi->k  *ein - sogi->X2;

    auto X1 = 积分( w0*errk , Ts ,  sogi->X1 );
    //auto X0 = 积分( w0*err0 , Ts ,  sogi->X0 );
    auto X2 = 积分( w0*X1   , Ts ,  sogi->X2 );

    sogi->X1 = X1;
    sogi->X2 = X2;
    sogi->err= ein;
    //sogi->X0 = X0;
}

/* 
//SOGI正交信号生成，包含直流抑制
static void SOGI正交信号生成( SOGI *sogi ,double x ,double w0 , double Ts ){
    //SOGI 对象数据更新在函数最后

    auto ein  = x -( sogi->X0 + sogi->X1 );
    auto err0 = sogi->k0 *ein;    
    auto errk  = sogi->k  *ein - sogi->X2;

    auto X1 = 积分( w0*errk , Ts ,  sogi->X1 );
    auto X0 = 积分( w0*err0 , Ts ,  sogi->X0 );
    auto X2 = 积分( w0*X1   , Ts ,  sogi->X2 );

    sogi->X1 = X1;
    sogi->X2 = X2;
    sogi->X0 = X0;
} */

//锁频环
static void Fll_Cale( FLL_Struct *p ,floatx ea , floatx qa ,floatx eb ,floatx qb ,floatx A , floatx w0 ,floatx k , floatx Ts)
{
    auto ef = 0.5*( ea*qa + eb*qb );

    //同幅值有关的自动调节系数
    auto auto_A = A*A;
    auto Amin_2 = (p->Amin)*(p->Amin);
    if( auto_A < Amin_2 ) auto_A = Amin_2;
    auto_A = 1.0/auto_A;
 
    //同角频率有关的自动调节系数，k 为 SOGI 中增益
    auto auto_w = ( p->w <0 )? 0 : p->w;
    auto_w = auto_w * k ; 

    //积分求Δw
    auto auto_g = auto_A*auto_w ;
    p->dw = 积分并限幅( - p->gama * ef * auto_g , Ts , p->dwmin , p->dwmax , p->dw );

    p->w =  p->dw + w0 ;
    if( p->w <0 ) p->w = 0;
    p->f = p->w /(2*π);

}

//正负序计算PNSC
static auto PNSC_Cale( floatx a ,floatx qa ,floatx b , floatx qb )
{
    auto apos = 0.5f*( a  - qb );
    auto bpos = 0.5f*( qa + b  );
    auto aneg = 0.5f*( qb + a  );
    auto bneg = 0.5f*( b  - qa );
    return std::make_tuple( apos , bpos , aneg , bneg );
}


// DSOGI_PNSC_FLL_PLL dsogi_pnsc_fll_pll = {
//     .PLL = {
//         .θ = 0,        
//         .f = 50,
//         .A = 0.01,
//         .f_lower = 36.16,
//         .PI_autoGain_min = 0.01,
//         .sPI1 = {
//             .kp = 94,
//             .ki = 3658,
//             .upper = 2 * π * 50,
//             .lower = -2 * π * 50,
//             .y_init = 0,
//             .yi = 0,
//             .y = 0},
//     },
//     .sfll = {
//         .f = 50,
//         .w = 2* π *50,
//         .dw = 0,
//         .gama = 30 ,
//         .Amin = 0.01 ,
//         .dwmax = 2 * π * 40 ,
//         .dwmin = -2 * π * 40 ,
//     },
//     .正序 =
//     {
//         .A = 0.01        
//     },
//     .负序 = {        
//     },
//     .A_αβ = 0.01,
//     .中间量 = {
//         .q_err = 0,
//     },
//     .sSOGIa = {
//         .k = 1.45,
//         .err = 0,
//         .X1 = 0,
//         .X2 = 0
//     },
//     .sSOGIb = {
//         .k = 1.45,
//         .err = 0,
//         .X1 = 0,
//         .X2 = 0
//     },
// };

DSOGI_PNSC_FLL_PLL *DSOGI_PNSC_FLL_PLL_Creat()
{
    try
    {
        auto p = new DSOGI_PNSC_FLL_PLL{
            .PLL = {
                .θ = 0,
                .f = 50,
                .A = 0.01,
                .f_lower = 36.16,
                .PI_autoGain_min = 0.01,
                .sPI1 = {
                    .kp = 94,
                    .ki = 3658,
                    .upper = 2 * π * 50,
                    .lower = -2 * π * 50,
                    .y_init = 0,
                    .yi = 0,
                    .y = 0},
            },
            .sfll = {
                .f = 50,
                .w = 2 * π * 50,
                .dw = 0,
                .gama = 30,
                .Amin = 0.01,
                .dwmax = 2 * π * 40,
                .dwmin = -2 * π * 40,
            },
            .正序 = {.A = 0.01},
            .负序 = {},
            .A_αβ = 0.01,
            .中间量 = {
                .q_err = 0,
            },
            .sSOGIa = {.k = 1.45, .err = 0, .X1 = 0, .X2 = 0},
            .sSOGIb = {.k = 1.45, .err = 0, .X1 = 0, .X2 = 0},

        };
        return p;
    }
    catch (const std::bad_alloc &e)
    {
        return nullptr;
    }
    
}



/*  DSOGI 锁相环,包括正负序计算，DSOGI所需角频率由FLL计算
    使用 自定义struct 类型全局变量
    ：方便基础软件部方式的调用，函数入口不传 自定义struct 类型参数
*/
void DSOGI_PNSC_FLL_PLL_Cal(
    DSOGI_PNSC_FLL_PLL *pll,
    float alpha, float beta, float f0,
    float Ts,
    float A_min4autogain, float dw_max, float dw_min,
    float SOGI_k, float fll_gama,
    float f_lower, float PI_kp, float PI_ki,
    float *wt_pll, float *A_pll, float *f_pll,
    float *f_fll,
    float *A_alpha_beta, float *A_pos,
    float *PosSeqAlpha, float *PosSeqBeta,
    float *NegSeqAlpha, float *NegSeqBeta,
    int reset)
{
    
    auto 输入系数 = [=](DSOGI_PNSC_FLL_PLL *p) { 
        //外部变量都隐式按值捕获
       
        p->sSOGIa.k = SOGI_k;
        p->sSOGIb.k = SOGI_k;
        
        p->sfll.gama = fll_gama;
        p->sfll.Amin = A_min4autogain;
        p->sfll.dwmax = dw_max;
        p->sfll.dwmin = dw_min;

        p->PLL.f_lower = f_lower ;
        p->PLL.PI_autoGain_min = A_min4autogain;
        p->PLL.sPI1.kp     = PI_kp;
        p->PLL.sPI1.ki     = PI_ki;
        p->PLL.sPI1.lower  = dw_min;
        p->PLL.sPI1.upper  = dw_max;
        p->PLL.sPI1.y_init = 0;
        
    };
    输入系数(pll);


    auto w0 = 2*π * f0 ; //初始角频率
    pll->A_αβ = abs(alpha,beta);//αβ得到的幅值

    if (reset)
    {   //非零复位

        //状态量复位
        pll->sfll.dw = 0;       
        pll->sSOGIa.X1 = 0;
        pll->sSOGIa.X2 = 0;
        pll->sSOGIb.X1 = 0;
        pll->sSOGIb.X2 = 0;
        pll->PLL.sPI1.y = 0;
        pll->PLL.sPI1.yi = 0;
        pll->PLL.θ = 0;
        pll->中间量.q_err = 0;
        //输出量
        pll->sfll.w = 0;
        pll->sfll.f = 0;
        pll->PLL.A = 0;
        pll->PLL.f = 0;
        pll->正序.A = 0;
        pll->正序.α = 0;
        pll->正序.β = 0;
        pll->负序.α = 0;
        pll->负序.β = 0;
    }
    else
    {   //不复位

        SOGI正交信号生成(&pll->sSOGIa, alpha, pll->sfll.w, Ts);
        SOGI正交信号生成(&pll->sSOGIb, beta, pll->sfll.w, Ts);
        Fll_Cale(&pll->sfll,
                 pll->sSOGIa.err, pll->sSOGIa.X2, pll->sSOGIb.err, pll->sSOGIb.X2,
                 std::max(pll->A_αβ, pll->正序.A),
                 w0, SOGI_k, Ts); //锁频环，其中自动增益的幅值：αβ和＋序的最大值

        std::tie(pll->正序.α, pll->正序.β, pll->负序.α, pll->负序.β) =
            PNSC_Cale(pll->sSOGIa.X1, pll->sSOGIa.X2, pll->sSOGIb.X1, pll->sSOGIb.X2); //正负序计算
        pll->正序.A = abs(pll->正序.α, pll->正序.β);

        auto [d, q] = αβ2dq(pll->正序.α, pll->正序.β, pll->PLL.θ);
        pll->PLL.A = fabs(d);                                                      //pll计算出的幅值
        auto Again = std::max(pll->PLL.A, (floatx)pll->PLL.PI_autoGain_min);       //PI环自动增益系数
        pll->中间量.q_err = 滤波_低通(q, Ts, pll->PLL.f_lower, pll->中间量.q_err); //环路滤波
        PI计算(&pll->PLL.sPI1, pll->中间量.q_err / Again, 0, Ts);                  //PI计算，暂不使用复位相关功能
        auto w = pll->PLL.sPI1.y + w0;
        w = std::max(w, 0.0);
        pll->PLL.θ = 积分_循环(w, Ts, 0, 2 * π, pll->PLL.θ);
        pll->PLL.f = f0 + pll->PLL.sPI1.y / (2 * π);
    }


    auto 输出结果 = [=]( float *pwt_pll , float *pA_pll , float *pf_pll , 
                                   float *pf_fll,
                                   float *pA_alpha_beta , float *pA_pos,
                                   float *pPosSeqAlpha , float *pPosSeqBeta,
                                   float *pNegSeqAlpha , float *pNegSeqBeta
                                 ) {
        //外部变量都隐式按值捕获
        *pwt_pll        =  pll->PLL.θ;
        *pA_pll         =  pll->PLL.A;
        *pf_pll         =  pll->PLL.f;
        *pf_fll         =  pll->sfll.f;
        *pA_alpha_beta  =  pll->A_αβ;
        *pA_pos         =  pll->正序.A;
        *pPosSeqAlpha   =  pll->正序.α;
        *pPosSeqBeta    =  pll->正序.β;
        *pNegSeqAlpha   =  pll->负序.α;
        *pNegSeqBeta    =  pll->负序.β;
    };
    输出结果( wt_pll,  A_pll,  f_pll,
                        f_fll,
                        A_alpha_beta,  A_pos,
                        PosSeqAlpha,  PosSeqBeta,
                        NegSeqAlpha,  NegSeqBeta);
}